#include "IRotation.h"

IRotation::IRotation(real angleVelocity, real facing) : angleVelocity(angleVelocity), facing(facing)
{
}

real IRotation::getAngleVelocity()
{
    return angleVelocity;
}

real IRotation::getFacing()
{
    return facing;
}

void IRotation::setAngleVelocity(real angleVelocity)
{
    this->angleVelocity = so_abs(angleVelocity);
}

void IRotation::setFacing(real facing)
{
    this->facing = so_angle(facing);
}

real IRotation::rotateTo(real delta, real targetFacing)
{
    targetFacing = so_angle(targetFacing);
    real av = delta * angleVelocity;
    real da = targetFacing - facing;
    real max = so_max(targetFacing, facing);
    real min = so_min(targetFacing, facing);
    real abs_a = max - min;
    real abs_b = 360 - max + min;
    real dangle = so_min(abs_a, abs_b);
    if (facing == targetFacing)
    {
        return 0;
    }
    if (da > 0)
    {
        av = abs_a > abs_b ? av : -av;
    }
    else
    {
        av = abs_a > abs_b ? -av : av;
    }
    if (dangle < so_abs(av))
    {
        facing = targetFacing;
        av = av > 0 ? dangle : -dangle;
    }
    else
    {
        facing = so_angle(facing - av);
    }
    return (dangle - av);
}
